#ifndef MY_LIFECYCLE_NODE_HPP_
#define MY_LIFECYCLE_NODE_HPP_

#include <behaviortree_cpp/bt_factory.h>
#include <iostream>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"      // 包含生命周期节点库
#include "lifecycle_msgs/msg/transition.hpp"        // 包含生命周期消息中的转换消息
#include "rclcpp_lifecycle/lifecycle_publisher.hpp" // 包含生命周期发布者库
#include "test_interfaces/srv/test.hpp"
#include "basic_lifecycle.h"
#include "status.h"

using namespace std::chrono_literals;
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;

/**
 * @brief  构建生命周期节点
 */
class MyLifecycleNode : public test_utils::BasicLifecycleNode
{
private:
    rclcpp::Client<test_interfaces::srv::Test>::SharedPtr client_;
    rclcpp::TimerBase::SharedPtr timer_;
    int sum = 0;
    bool next_goal = true;
    std::queue<int> request_list;
    // rclcpp::WaitSet wait_set_;

public:
    MyLifecycleNode(const std::string &name) : test_utils::BasicLifecycleNode(name, rclcpp::NodeOptions())
    {
    }

    ~MyLifecycleNode()
    {
    }

    CallbackReturn on_configure(const rclcpp_lifecycle::State &)
    {
        client_ = this->create_client<test_interfaces::srv::Test>("add_three_ints");
        this->timer_ = this->create_wall_timer(
            std::chrono::milliseconds(100),
            std::bind(&MyLifecycleNode::my_loop, this));

        // wait_set_.add_client(client_);
        RCLCPP_INFO(this->get_logger(), "on_configure() is called.");
        return CallbackReturn::SUCCESS;
    }

    CallbackReturn on_activate(const rclcpp_lifecycle::State &state)
    {
        // 父类方法自动在托管实体（目前是LifecyclePublisher）上过渡。
        // 这里也可以手动调用pub_->on_activate()。
        // 重写此方法是可选的，很多时候默认方法就足够了。
        LifecycleNode::on_activate(state);

        RCLCPP_INFO(this->get_logger(), "on_activate() is called.");
        return CallbackReturn::SUCCESS;
    }

    CallbackReturn on_deactivate(const rclcpp_lifecycle::State &state)
    {
        LifecycleNode::on_deactivate(state);

        RCLCPP_INFO(this->get_logger(), "on_deactivate() is called.");
        return CallbackReturn::SUCCESS;
    }

    CallbackReturn on_cleanup(const rclcpp_lifecycle::State &)
    {
        RCLCPP_INFO(this->get_logger(), "on cleanup is called.");
        return CallbackReturn::SUCCESS;
    }

    CallbackReturn on_shutdown(const rclcpp_lifecycle::State &state)
    {
        RCLCPP_INFO(this->get_logger(), "on shutdown is called from state %s.", state.label().c_str());
        return CallbackReturn::SUCCESS;
    }

    // 定时器回调
    void my_loop()
    {
        if (this->next_goal && !this->request_list.empty())
        {
            // 暂停后续请求
            this->next_goal = false;

            // 发送此次请求
            auto request = std::make_shared<test_interfaces::srv::Test::Request>();
            request->a = 1;
            request->b = 2;
            request->c = 3;

            client_->async_send_request(request, std::bind(&MyLifecycleNode::callback, this, std::placeholders::_1));
            RCLCPP_INFO(this->get_logger(), "sended request");
        }
    }

    void send_request()
    {
        this->request_list.push(1);
    }

    void callback(rclcpp::Client<test_interfaces::srv::Test>::SharedFuture future)
    {
        // 处理请求结果
        auto result = future.get();
        sum = result->sum;
        RCLCPP_INFO(this->get_logger(), "got response");
        RCLCPP_INFO(this->get_logger(), "result is %d", sum);

        // 放开后续请求
        this->request_list.pop();
        this->next_goal = true;
    }

    bool isFinished()
    {
        if (!rclcpp::ok())
        {
            RCLCPP_ERROR(this->get_logger(), "node shutdown");
            return true;
        }

        if (this->request_list.empty())
        {
            return true;
        }
        else
        {
            return false;
        }

        // auto wait_result = wait_set_.wait(std::chrono::seconds(0));
        // if (wait_result.kind() == rclcpp::WaitResultKind::Ready)
        // {
        //     if (wait_result.get_wait_set().get_rcl_wait_set().clients[0])
        //     {
        //         RCLCPP_INFO(this->get_logger(), "Wait-set handle client");
        //         test_interfaces::srv::Test::Response response;
        //         rmw_request_id_t request_id;
        //         if (client_->take_response(response, request_id))
        //         {
        //             sum = response.sum;
        //         }
        //         return true;
        //     }
        // }
        // else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout)
        // {
        //     RCLCPP_ERROR(this->get_logger(), "Wait-set failed with timeout");
        //     return false;
        // }
        // else
        // {
        //     return false;
        // }
    }

    int get_result()
    {
        return sum;
    }
};

#endif